#include "filter.h"
#include "structconfig.h"
#include "math.h"


void Filter_Init(void)
{
	Butterworth_Parameter_Init();
}

/*******************************************************************************
*函  数 ：void Hubu__Filter(float IN_Gaopin,float IN_Dipin,float Out,float a)
*功  能 ：一阶互补滤波
*参  数 ：IN_Gaopin  具有高频噪声的校正量
*         IN_Dipin	 具有低频噪声既存在零偏的积分量
*         Out				 互补滤波后的输出值
					a					 对校正量的信任参数，通常取0.01-0.1
					trans			 单位转化参数，普通互补时为1，加速度计由米转化厘米时为100，陀螺仪由弧度转角度时为57.295780f
*返回值 ：
*备  注 : 无
*******************************************************************************/
void Hubu_Filter(float IN_Gaopin,float IN_Dipin,float *Out,float a,float trans)
{
	*Out = a * IN_Gaopin + (1-a) * (*Out + IN_Dipin * 0.01f*trans);
}

/*******************************************************************************
*函  数 ：void RC_Filter(float IN,float *Out,float a)
*功  能 ：一阶RC低通滤波
*参  数 	IN    具有高频噪声的输入
          Out		滤波后的输出
					a			滤波参数，参数越小滤波效果越好，相位延迟越大
*返回值 ：
*备  注 : 无
*******************************************************************************/
void RC_Filter(float IN,float *Out,float a)
{
	*Out=(1-a)*(*Out)+a*IN;
}

Butter_Parameter Butter_80HZ_Parameter_Acce,Butter_60HZ_Parameter_Acce,Butter_51HZ_Parameter_Acce,
								 Butter_30HZ_Parameter_Acce,Butter_20HZ_Parameter_Acce,Butter_15HZ_Parameter_Acce,
								 Butter_10HZ_Parameter_Acce,Butter_5HZ_Parameter_Acce,Butter_2HZ_Parameter_Acce,
								 Butter_5HZ_Parameter_Gyro,Butter_4HZ_Parameter_Gyro,Butter_2HZ_Parameter_Gyro,
								 Butter_10HZ_Parameter_Opt,Butter_5HZ_Parameter_Opt,Butter_2HZ_Parameter_Opt,
								 Butter_5HZ_Parameter_AccDif,Butter_5HZ_Parameter_US;

/*************************************************
函数名:	float LPButterworth(float curr_input,Butter_BufferData *Buffer,Butter_Parameter *Parameter)
说明:	加速度计低通滤波器
入口:	float curr_input 当前输入加速度计,滤波器参数，滤波器缓存
出口:	无
备注:	2阶Butterworth低通滤波器
*************************************************/
float LPButterworth(float curr_input,Butter_BufferData *Buffer,Butter_Parameter *Parameter)
{
	uint16_t i;
  /* 加速度计Butterworth滤波 */
  /* 获取最新x(n) */
  Buffer->Input_Butter[2]=(float)(curr_input);
  /* Butterworth滤波 */
  Buffer->Output_Butter[2]=
    Parameter->b[0] * Buffer->Input_Butter[2]
      +Parameter->b[1] * Buffer->Input_Butter[1]
        +Parameter->b[2] * Buffer->Input_Butter[0]
          -Parameter->a[1] * Buffer->Output_Butter[1]
            -Parameter->a[2] * Buffer->Output_Butter[0];
  /* x(n) 序列保存 */
  Buffer->Input_Butter[0]=Buffer->Input_Butter[1];
  Buffer->Input_Butter[1]=Buffer->Input_Butter[2];
  /* y(n) 序列保存 */
  Buffer->Output_Butter[0]=Buffer->Output_Butter[1];
  Buffer->Output_Butter[1]=Buffer->Output_Butter[2];
	for(i=0;i<3;i++)
	{
	  if(isnan(Buffer->Output_Butter[i])==1) 
			Buffer->Output_Butter[i]=curr_input;
		if(isnan(Buffer->Input_Butter[i])==1)  
			Buffer->Input_Butter[i]=curr_input;
	}
  return Buffer->Output_Butter[2];
}

/****************************************
Butterworth低通滤波器参数初始化：http://blog.csdn.net/u011992534/article/details/73743955
***************************************/
/***********************************************************
@函数名：Butterworth_Parameter_Init
@入口参数：无
@出口参数：无
功能描述：巴特沃斯低通滤波器初始化
*************************************************************/
void Butterworth_Parameter_Init(void)
{
  Set_Cutoff_Frequency(Imu_Sampling_Freq, 80,&Butter_80HZ_Parameter_Acce);
  Set_Cutoff_Frequency(Imu_Sampling_Freq, 60,&Butter_60HZ_Parameter_Acce); 
  Set_Cutoff_Frequency(Imu_Sampling_Freq, 51,&Butter_51HZ_Parameter_Acce);
  Set_Cutoff_Frequency(Imu_Sampling_Freq, 30,&Butter_30HZ_Parameter_Acce);
  Set_Cutoff_Frequency(Imu_Sampling_Freq, 20,&Butter_20HZ_Parameter_Acce);
  Set_Cutoff_Frequency(Imu_Sampling_Freq, 15,&Butter_15HZ_Parameter_Acce); 
  Set_Cutoff_Frequency(Imu_Sampling_Freq, 10,&Butter_10HZ_Parameter_Acce);
  Set_Cutoff_Frequency(Imu_Sampling_Freq, 5 ,&Butter_5HZ_Parameter_Acce);
  Set_Cutoff_Frequency(Imu_Sampling_Freq, 2 ,&Butter_2HZ_Parameter_Acce);
	Set_Cutoff_Frequency(Imu_Sampling_Freq, 10 ,&Butter_5HZ_Parameter_Gyro);
	Set_Cutoff_Frequency(Imu_Sampling_Freq, 4.7 ,&Butter_4HZ_Parameter_Gyro);
	Set_Cutoff_Frequency(Imu_Sampling_Freq, 2 ,&Butter_2HZ_Parameter_Gyro);
	Set_Cutoff_Frequency(Imu_Sampling_Freq, 10 ,&Butter_10HZ_Parameter_Opt);
	Set_Cutoff_Frequency(Imu_Sampling_Freq, 5 ,&Butter_5HZ_Parameter_Opt);
	Set_Cutoff_Frequency(Imu_Sampling_Freq, 5 ,&Butter_5HZ_Parameter_AccDif);
	Set_Cutoff_Frequency(Imu_Sampling_Freq, 5 ,&Butter_5HZ_Parameter_US);

}

//https://blog.csdn.net/sszhouplus/article/details/43113505
//https://blog.csdn.net/shengzhadon/article/details/46784509
//https://blog.csdn.net/shengzhadon/article/details/46791903
//https://blog.csdn.net/shengzhadon/article/details/46803401
/***********************************************************
@函数名：Set_Cutoff_Frequency
@入口参数：float sample_frequent, float cutoff_frequent,
Butter_Parameter *LPF
@出口参数：无
功能描述：巴特沃斯低通滤波器初始化
*************************************************************/
void Set_Cutoff_Frequency(float sample_frequent, float cutoff_frequent,Butter_Parameter *LPF)
{
  float fr = sample_frequent / cutoff_frequent;
  float ohm = tanf(M_PI_F / fr); 
  float c = 1.0f + 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm;
  if (cutoff_frequent <= 0.0f) {
    // no filtering
    return;
  }
  LPF->b[0] = ohm * ohm / c;
  LPF->b[1] = 2.0f * LPF->b[0];
  LPF->b[2] = LPF->b[0];
  LPF->a[0]=1.0f;
  LPF->a[1] = 2.0f * (ohm * ohm - 1.0f) / c;
  LPF->a[2] = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;
}

////////////////////////////一阶卡尔曼滤波/////////////////////
void kalman_1(float in,float *out)  
{
	static float out_1,K,P,p;
	p = P + 0.1f; //Q=0.1f
	K = p / (p + 0.2f); //R=0.2f
	*out = out_1 + K * (in - out_1);
	out_1=*out;
	P = (1-K) * p ;
}



#define N 20      //滤波缓存数组大小

/*******************************************************************************
*函  数 ：float FindPos(float*a,int low,int high)
*功  能 ：确定一个元素位序
*参  数 ：a  数组首地址
*         low数组最小下标
*         high数组最大下标
*返回值 ：返回元素的位序low
*备  注 : 无
*******************************************************************************/
float FindPos(float*a,int low,int high)
{
    float val = a[low];                      //选定一个要确定值val确定位置
    while(low<high)
    {
        while(low<high && a[high]>=val)
             high--;                       //如果右边的数大于VAL下标往前移
             a[low] = a[high];             //当右边的值小于VAL则复值给A[low]

        while(low<high && a[low]<=val)
             low++;                        //如果左边的数小于VAL下标往后移
             a[high] = a[low];             //当左边的值大于VAL则复值给右边a[high]
    }
    a[low] = val;//
    return low;
}

/*******************************************************************************
*函  数 ：void QuiteSort(float* a,int low,int high)
*功  能 ：快速排序
*参  数 ：a  数组首地址
*         low数组最小下标
*         high数组最大下标
*返回值 ：无
*备  注 : 无
*******************************************************************************/
 void QuiteSort(float* a,int low,int high)
 {
     int pos;
     if(low<high)
     {
         pos = FindPos(a,low,high); //排序一个位置
         QuiteSort(a,low,pos-1);    //递归调用
         QuiteSort(a,pos+1,high);
     }
 }

 /*******************************************************************************
*函  数 ：float GildeAverageValueFilter_MAG(float NewValue,float *Data,uint8_t n)
*功  能 ：去最值平均值滤波一组数据
*参  数 ：value 采样的数据
*返回值 ：返回滤波后的数据
 *备  注 : 无
*******************************************************************************/
float GildeAverageValueFilter_MAG(float NewValue,float *Data,uint8_t n)
{
  float max,min;
  float sum;
  uint8_t i;
  Data[0]=NewValue;
  max=Data[0];
  min=Data[0];
  sum=Data[0];
  for(i=n-1;i!=0;i--)
  {
    if(Data[i]>max) max=Data[i];
    else if(Data[i]<min) min=Data[i];
    sum+=Data[i];
    Data[i]=Data[i-1];
  }
  i=n-2;
  sum=sum-max-min;
  sum=sum/i;
  return(sum);
}



/*******************************************************************************
*函  数 ：void  SortAver_FilterXYZ(INT16_XYZ *acc,FLOAT_XYZ *Acc_filt,uint8_t n)
*功  能 ：去最值平均值滤波三组数据
*参  数 ：*acc 要滤波数据地址
*         *Acc_filt 滤波后数据地址
*返回值 ：返回滤波后的数据
*备  注 : 无
*******************************************************************************/
void  SortAver_FilterXYZ(INT16_XYZ *acc,FLOAT_XYZ *Acc_filt,uint8_t n)
{
	static float bufx[N],bufy[N],bufz[N];
	static uint8_t cnt =0,flag = 1;
	float temp1=0,temp2=0,temp3=0;
	uint8_t i;
	bufx[cnt] = acc->X;
	bufy[cnt] = acc->Y;
	bufz[cnt] = acc->Z;
	cnt++;      //这个的位置必须在赋值语句后，否则bufx[0]不会被赋值
	if(cnt<n && flag) 
		return;   //数组填不满不计算
	else
		flag = 0;
	
  QuiteSort(bufx,0,n-1);
	QuiteSort(bufy,0,n-1);
	QuiteSort(bufz,0,n-1);
	for(i=1;i<n-1;i++)
	 {
		temp1 += bufx[i];
		temp2 += bufy[i];
		temp3 += bufz[i];
	 }

	 if(cnt>=n) cnt = 0;
	 Acc_filt->X  = temp1/(n-2);
	 Acc_filt->Y  = temp2/(n-2);
	 Acc_filt->Z  = temp3/(n-2);
}


/*******************************************************************************
*函  数 ：void Aver_FilterXYZ(INT16_XYZ *acc,FLOAT_XYZ *Acc_filt,uint8_t n)
*功  能 ：滑动窗口滤波三组数据
*参  数 ：*acc  要滤波数据地址
*         *Acc_filt 滤波后数据地址
*返回值 ：返回滤波后的数据
*备  注 : 无
*******************************************************************************/
void Aver_FilterXYZ(INT16_XYZ *acc,FLOAT_XYZ *Acc_filt,uint8_t n)
{
  static int32_t bufax[N],bufay[N],bufaz[N];
	static uint8_t cnt =0,flag = 1;
	int32_t temp1=0,temp2=0,temp3=0,i;
	bufax[cnt] = acc->X;
	bufay[cnt] = acc->Y;
	bufaz[cnt] = acc->Z;
	cnt++;      //这个的位置必须在赋值语句后，否则bufax[0]不会被赋值
	if(cnt<n && flag) 
		return;   //数组填不满不计算
	else
		flag = 0;
	for(i=0;i<n;i++)
	{
		temp1 += bufax[i];
		temp2 += bufay[i];
		temp3 += bufaz[i];
	}
	 if(cnt>=n)  cnt = 0;
	 Acc_filt->X  = temp1/n;
	 Acc_filt->Y  = temp2/n;
	 Acc_filt->Z  = temp3/n;
}

/*******************************************************************************
*函  数 ：void Aver_Filter(float data,float *filt_data,uint8_t n
*功  能 ：滑动窗口滤波一组数据
*参  数 ：data  要滤波数据
*         *filt_data 滤波后数据地址
*返回值 ：返回滤波后的数据
*备  注 : 无
*******************************************************************************/
void Aver_Filter(float data,float *filt_data,uint8_t n)
{
  static float buf[N];
	static uint8_t cnt =0,flag = 1;
	float temp=0;
	uint8_t i;
	buf[cnt] = data;
	cnt++;
	if(cnt<n && flag) 
		return;   //数组填不满不计算
	else
		flag = 0;
	
	for(i=0;i<n;i++)
	{
		temp += buf[i];
	}
	if(cnt>=n) cnt = 0;
	 *filt_data = temp/n;
}






